import rclpy
from rclpy.node import Node
import rclpy.time
import tf2_ros
from geometry_msgs.msg import Twist
from tf2_ros import TransformListener, Buffer
from sensor_msgs.msg import Range
import math
from rclpy.time import Time
import serial
from std_msgs.msg import String
from yolo_msg.msg import Yolo
import time

class Control(Node):
    def __init__(self):
        super().__init__('control')
        # 创建 tf2 缓存和监听器
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 启动定时器周期性的获取 tf 变换
        self.timer = self.create_timer(0.001, self.timer_callback)
        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.height_sub = self.create_subscription(Range, '/height', self.height_callback, 10)
        self.control_sub = self.create_subscription(String, '/control', self.control_callback, 10)
        self.face_sub = self.create_subscription(Yolo, '/face_drone', self.face_callback, 10)
        self.log_timer = self.create_timer(1, self.log_callback)


        self.MAX_LINEAR_SPEED = 25             #最大线速度，单位 cm/s
        self.MAX_ANGULAR_SPEED = 15             #最大角速度,单位 度/s

        self.KP_LINEAR_SPEED_POINT = 0.8        #坐标模式线速度KP
        self.KP_LINEAR_SPEED_DETECT = 0.055      #识别模式线速度KP
        self.KP_ANGULAR_SPEED = 0               #角速度环KP

        self.KP_Z_SPEED = 1
        self.MAX_Z_SPEED = 40

        self.MIN_ERROR_XY_POINT = 5             #坐标模式XY方向允许的最小误差，单位 cm
        self.MIN_ERROR_XY_DETECT = 50         #识别模式XY方向允许的最小误差，单位 像素
        self.MIN_ERROR_YAW = 25                 #YAW上最小角度误差，单位 度、

        ####定义变量####

        #目标变量
        self.targetPointX = 0                   #目标坐标：X，单位 cm
        self.targetPointY = 0                   #目标坐标：Y，单位 cm
        self.targetPointZ = 0                   #目标坐标：Z，单位 cm
        self.targetDetectX = 320                #识别模式X方向目标像素 单位 像素
        self.targetDetectY = 0                #识别模式Y方向目标像素 单位 像素
        self.targetDetectZ = 200                  #识别模式Z方向目标像素 单位 像素
        self.targetYaw = 0                      #目标角度YAW， 单位度
        self.targetNum = 8                      #目标数字
        self.targetHeight = 75                  #目标高度

        #中间变量
        self.distanceXY = 0                     #坐标模式距离目标点距离，单位 cm
        self.errorPointX = 0                    #坐标模式X方向偏差像素值，单位 cm
        self.errorPointY = 0                    #坐标模式Y方向偏差像素值，单位 cm
        self.errorPointZ = 0                    #坐标模式Z方向偏差像素值，单位 cm
        self.errorDetectX = 0                   #识别模式X方向偏差像素值，单位 像素
        self.errorDetectY = 0                   #识别模式Y方向偏差像素值，单位 像素
        self.errorDetectZ = 0                   #识别模式Z方向偏差像素值，单位 像素
        self.actualPositionX = 0                #实际位置x
        self.actualPositionY = 0                #实际位置y  
        self.actualPositionZ = 0                #实际位置z
        self.actualPositionYaw = 0              #实际位置YAW
        self.detectX = 0                        #识别到数字中心X坐标
        self.detectY = 0                        #识别到数字中心Y坐标
        self.detectZ = 0                        #识别到数字中心Z坐标
        self.errorYaw = 0                       #Yaw方向误差值，单位 度
        self.cosTheta = 0                       #目标坐标角度余弦值
        self.sinTheta = 0                       #目标坐标角度正弦值
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Delay = 0                #延时开启时间
        self.presentTime_Delay = 0              #延时当前时间
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Height = 0               #高度变换开启时间
        self.presentTime_Height = 0             #高度变换当前时间
        

        #结果变量
        self.resultLinearSpeedXY = 0            #计算输出XY合速度
        self.resultLinearSpeedX = 0             #计算输出X方向速度
        self.resultLinearSpeedY = 0             #计算输出Y方向速度
        self.resultLinearSpeedZ = 0             #计算输出Z方向速度
        self.resultAngularSpeedYaw = 0          #计算输出Yaw方向角速度
        


        #状态变量
        self.flagMovePoint = -1                  #坐标飞行标志
        self.flagdetect = 0                     #目标点对应的flag
        self.mode = 0                           #飞行模式
        self.detectNum = -1                     #识别到的数字
        self.flagLaser = 0                      #激光灯状态变量，0为关闭， 1为开启
        self.flagDelay = 0                      #延时的状态
        self.flagHeight = 0                     #高度变化的状态
        self.flagland = 0                       #降落状态
        self.flagfly = 0                        #起飞状态

        self.startTime_Delay = 0

        self.Time_0 = 0
        self.Time = 0
        self.sleep_Time0 = 0
        self.sleep_Time = 25
       
        self.stringMax = ""
        self.stringMin = ""
        self.min_data = -1
        self.max_data = -1

        #绕杆数据
        self.r = -1
        self.angle = None
        self.raogan_state = 0
        #当开始进入识别二维码区域时,若向前飞且在0侧置1， 向后飞且在1侧置2， 向前飞且在1侧置3， 向后飞且在0侧置4， 其余为0
        self.detect_flag = 0
        self.raogan_judge_angle = 30#扫描最小角度
        self.raogan_x = -1
        self.raogan_y = -1
        
        self.raogan_distance = 40

        self.flagface = 0
        self.facex = 0
        self.facey = 0
        self.goring = 0


        self.ser = serial.Serial('/dev/ttyUSB0', 115200)
        
        self.ser.write("@0\r\n".encode())

        self.kuan = 10.8
        


        self.control_flag = 0
        self.flagMovePoint = -1      
        self.contrl_str = ""


    def tf2_euler_from_quaternion(self, quat):
        
        x, y, z, w = quat
        roll = math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
        pitch = math.asin(2.0 * (w * y - z * x))
        yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
        
        return [roll, pitch, yaw]  # 返回欧拉角 (roll, pitch, yaw)

    def pub_cmd_vel(self, x=0, y=0, z=0, yaw=0):
        msg = Twist()
        msg.linear.x = x * 0.01
        msg.linear.y = y * 0.01
        msg.linear.z = z * 0.01
        msg.angular.z = yaw * math.pi / 180

        self.cmd_vel_publisher.publish(msg)

    def calculate_point_speed(self):

        
        #计算距离
        self.distanceXY = math.sqrt(self.errorPointX * self.errorPointX + self.errorPointY * self.errorPointY)

        #计算合速度大小
        self.resultLinearSpeedXY = self.KP_LINEAR_SPEED_POINT * self.distanceXY 
        self.resultLinearSpeedXY = max(min(self.resultLinearSpeedXY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)

        #计算三角函数
        if self.distanceXY != 0:
            self.cosTheta = self.errorPointX / (self.distanceXY)
            self.sinTheta = self.errorPointY / (self.distanceXY)


        #计算输出速度
        self.resultLinearSpeedX = self.resultLinearSpeedXY * self.cosTheta
        self.resultLinearSpeedY = self.resultLinearSpeedXY * self.sinTheta
        self.resultAngularSpeedYaw = self.KP_ANGULAR_SPEED * self.errorYaw

        self.resultLinearSpeedZ = self.KP_Z_SPEED * self.errorPointZ
        
        #速度限幅
        self.resultAngularSpeedYaw = max(min(self.resultAngularSpeedYaw, self.MAX_ANGULAR_SPEED), -self.MAX_ANGULAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_Z_SPEED), -self.MAX_Z_SPEED)


    def face_callback(self, msg):
        pass
        # if self.flagface == 1:
        #     x = msg.x
        #     y = msg.y
        #     dx = 280 - y
        #     dy = 320 - x

        #     try:
        #         transform = self.tf_buffer.lookup_transform(
        #         'map',     # 目标坐标系
        #         'laser',    # 源坐标系
        #         rclpy.time.Time())  # 当前时间
            
        #         rotation = transform.transform.rotation
        #         quat = [rotation.x, rotation.y, rotation.z, rotation.w]
        #         eular = self.tf2_euler_from_quaternion(quat)
        #         self.actualPositionYaw = eular[2] * 180 / math.pi
        #         self.actualPositionX = transform.transform.translation.x * 100
        #         self.actualPositionY = transform.transform.translation.y * 100
        #     except:
        #         pass
            
        #     # self.facex = self.actualPositionX + 0.95 * dx 
        #     # self.facey = self.actualPositionY + 0.95 * dy
        #     # self.facex = 63
        #     # self.facey = 138
        #     self.flagface = 0
        #    # self.flagMovePoint = 1000


    #计算识别模式速度
    def calculate_detect_speed(self, ID):
        #计算误差像素
        if (ID == 0):
            self.errorDetectX = -(self.targetDetectX - self.detectX)
        elif (ID == 1):
            self.errorDetectX = (self.targetDetectX - self.detectX)
        #self.errorDetectY = self.targetDetectY - self.detectY
        self.errorDetectZ = (self.targetDetectZ - self.detectZ)
        #计算识别后速度
        self.resultLinearSpeedX = self.errorDetectX * self.KP_LINEAR_SPEED_DETECT
        #self.resultLinearSpeedY = self.errorDetectY * self.KP_LINEAR_SPEED_DETECT
        self.resultLinearSpeedZ = self.errorDetectZ * self.KP_LINEAR_SPEED_DETECT
        #计算限速
        self.resultLinearSpeedX = max(min(self.resultLinearSpeedX, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        #self.resultLinearSpeedY = max(min(self.resultLinearSpeedY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
    

    #坐标移动,更改目标坐标点
    #返回为真代表已经到达目的地
    def move(self, x, y, z, yaw=0, ignore_z = 0):
        x_act, y_act, yaw_act = self.actualPositionX, self.actualPositionY, self.actualPositionYaw
        z_act = self.actualPositionZ
        self.targetPointX = x
        self.targetPointY = y
        self.targetPointZ = z
        self.targetYaw = yaw
        self.errorPointX = self.targetPointX - x_act   
        self.errorPointY = self.targetPointY - y_act
        self.errorPointZ = self.targetPointZ - z_act
        self.errorYaw = self.targetYaw - yaw_act
        if ignore_z == 0:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                    abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        else:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= 10)
        
    
    #判断是否到达目的地
    def judge_point(self):
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        
    #判断是否到达识别中心
    def judge_detect(self):
        return (abs(self.errorDetectX) <= self.MIN_ERROR_XY_DETECT)
    
    def Delay(self, time):
        if self.flagDelay == 0:
            self.flagDelay = 1
            self.startTime_Delay = rclpy.time()
            self.resultLinearSpeedX = 0
            self.resultLinearSpeedY = 0
            self.resultLinearSpeedZ = 0
            self.mode = -1
            return 0
        elif self.flagDelay == 1:
            self.presentTime_Delay = rclpy.clock.Clock().now()
            print("WAITING")
            if (self.presentTime_Delay - self.startTime_Delay >= time):
                print("Delay Over")
                self.flagDelay = 0
                self.mode = 0
                return 1
            else:
                return 0
    
    
    def height_callback(self, msg):
        self.actualPositionZ = msg.range * 100
    
    def control_callback(self, msg):
        print("res:", msg.data)
        self.contrl_str = msg.data
        if self.control_flag == 0 and msg.data == '10':
            self.fly_up()
            self.fly_up()
            self.fly_up()
            self.flagMovePoint = 0
            self.control_flag = 1
            print("fly,10")
            #吸救生圈
            self.ser.write("@9\r\n".encode())
            self.ser.write("@2\r\n".encode())
            time.sleep(2)
            self.ser.write("@0\r\n".encode())
        if self.control_flag == 0 and msg.data == '11':
            self.fly_up()
            self.fly_up()
            self.fly_up()
            self.flagMovePoint = 100
            self.control_flag = 1
            print("fly,11")
            #吸救生圈
            self.ser.write("@9\r\n".encode())
            self.ser.write("@2\r\n".encode())
            time.sleep(2)
            self.ser.write("@0\r\n".encode())
        if self.control_flag == 0 and msg.data == '20':
            self.fly_up()
            self.fly_up()
            self.fly_up()
            self.flagMovePoint = 0
            self.control_flag = 1
            print("fly,20")
        if self.control_flag == 0 and msg.data == '21':
            self.fly_up()
            self.fly_up()
            self.fly_up()
            self.flagMovePoint = 100
            self.control_flag = 1
            print("fly,21")

        
        
    def fly_up(self):
        for i in range(5):
            self.pub_cmd_vel(0, 0, 100, 0)
            
        
    def land(self):
        for i in range(5):
            self.pub_cmd_vel(0, 0, -100, 0)
            

    def timer_callback(self):
        try:
            transform = self.tf_buffer.lookup_transform(
                'map',     # 目标坐标系
                'laser',    # 源坐标系
                rclpy.time.Time())  # 当前时间
            
            rotation = transform.transform.rotation
            quat = [rotation.x, rotation.y, rotation.z, rotation.w]
            eular = self.tf2_euler_from_quaternion(quat)
            self.actualPositionYaw = eular[2] * 180 / math.pi
            self.actualPositionX = transform.transform.translation.x * 100
            self.actualPositionY = transform.transform.translation.y * 100
            

 #具体控制写在这#
#############################################___CONTROL_CODE_BEGIN___###############################################################

            #0号摄像头巡航
            if self.flagMovePoint == 0 and self.move(0, 0, 100):
                self.flagMovePoint = 1
            elif self.flagMovePoint == 1 and self.move(8 * self.kuan, -1 * self.kuan, 100):
                self.flagMovePoint = 2
                #self.flagface = 1
            elif self.flagMovePoint == 2 and self.move(7.5 * self.kuan, 3.5 * self.kuan, 100):
                self.flagMovePoint = 1001
                self.ser.write('@1\r\n'.encode())
            # elif self.flagMovePoint == 2 and self.move(7 * self.kuan, 3 * self.kuan, 100):
            #     self.flagMovePoint = 3
            # elif self.flagMovePoint == 3 and self.move(7 * self.kuan, 7 * self.kuan, 100):
            #     self.flagMovePoint = 1
           

            #1号摄像头巡航
            if self.flagMovePoint == 100 and self.move(0, 0, 100):
                self.flagMovePoint = 101
            elif self.flagMovePoint == 101 and self.move(6 * self.kuan, 10 * self.kuan, 100):
                self.flagMovePoint = 102
                self.flagface = 0
            elif self.flagMovePoint == 102 and self.move(5 * self.kuan, 12 * self.kuan, 100):
                self.flagMovePoint = 1000
            elif self.flagMovePoint == 103 and self.move(5 * self.kuan, 17 * self.kuan, 100):
                self.flagMovePoint = 104
            elif self.flagMovePoint == 104 and self.move(6 * self.kuan, 19 * self.kuan, 100):
                self.flagMovePoint = 100


            
            #找到人脸救人
            elif self.flagMovePoint == 1000 and self.move(7.5 * self.kuan, 3.5 * self.kuan, 100):
                self.flagMovePoint = 1001
                self.ser.write('@1\r\n'.encode())
            elif self.flagMovePoint == 1001 and self.move(7.5 * self.kuan, 3.5 * self.kuan, 20):
                self.flagMovePoint = 1002
                self.ser.write('@8\r\n'.encode())
                self.ser.write('@0\r\n'.encode())
                self.ser.write('@0\r\n'.encode())
            
            elif self.flagMovePoint == 1002 and self.move(7.5 * self.kuan, 3.5 * self.kuan, 100):
                self.flagMovePoint = 1003
                if self.contrl_str == "10" or self.contrl_str == "11":
                    self.flagMovePoint = 666
                if self.contrl_str == "20" or self.contrl_str == "21":
                    self.flagMovePoint = 888
                
            


            #降落
            elif self.flagMovePoint == 666 and self.move(0, 0, 100):
                self.flagMovePoint = 667
            elif self.flagMovePoint == 667 and self.move(0, 0, 5):
                self.land()
                self.flagMovePoint = -1
                self.control_flag = 0
                self.flagface = 1

            elif self.flagMovePoint == 888 and self.move(17 * self.kuan, 11 * self.kuan, 100):
                self.flagMovePoint = 889
            elif self.flagMovePoint == 889 and self.move(20 * self.kuan, 11 *  self.kuan, 100):
                self.flagMovePoint = 890
            elif self.flagMovePoint == 890 and self.move(28.5 * self.kuan, 11 *  self.kuan, 100):
                self.flagMovePoint = 891
                self.ser.write('@1\r\n'.encode())
            elif self.flagMovePoint == 891 and self.move(28.5 * self.kuan, 11 *  self.kuan, 20):
                self.flagMovePoint = 892
                self.ser.write('@0\r\n'.encode())
            elif self.flagMovePoint == 892 and self.move(28.5 * self.kuan, 11 *  self.kuan, 100):
                self.flagMovePoint = 893
            elif self.flagMovePoint == 893 and self.move(20 * self.kuan, 11 *  self.kuan, 100):
                self.flagMovePoint = 894
            elif self.flagMovePoint == 894 and self.move(17 * self.kuan, 11 * self.kuan, 100):
                self.flagMovePoint = 666





        

#############################################___CONTROL_CODE_END___################################################################

            self.calculate_point_speed()

            self.pub_cmd_vel(self.resultLinearSpeedX, self.resultLinearSpeedY, self.resultLinearSpeedZ, self.resultAngularSpeedYaw)
        except tf2_ros.TransformException as e:
            self.get_logger().warn("No TF")







    def log_callback(self):
        self.get_logger().info(f"x: {self.actualPositionX:.3f}, y: {self.actualPositionY:.3f}, z:{self.actualPositionZ:.3f}, yaw:{self.actualPositionYaw:.3f}")
        self.get_logger().info(f"x: {self.resultLinearSpeedX:f}, y: {self.resultLinearSpeedY:f}, z:{self.resultLinearSpeedZ:f}")
        self.get_logger().info(f"flag: {self.flagMovePoint:d}")
        self.get_logger().info("bbb")

        


def main(args=None):
    rclpy.init(args=args)
    node = Control()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
